//
// Created by whitby on 6/1/23.
//

#include <iostream>
#include <fstream>
#include <vector>
#include <opencv2/opencv.hpp>

using namespace std;

void loadTimeStamps(const string &path, vector<double> &timestamps);

void loadIMU(const string &path, vector<double> &timestamps, vector<cv::Point3f> &vecAcc, vector<cv::Point3f> &vecGyro);

void checkTimeStamps(const vector<double> &times, const int &frequency) {
    double delta_t = 1.0 / frequency;
    int n = int(times.size());
    cout.precision(16);
    for (int i = 1; i < n; ++i) {
        if (times[i] > times[i - 1] && times[i] < times[i - 1] + delta_t * 8)
            continue;
        else {
            cout << " - iter: " << i << " ,illegal time: " << times[i-1] << " - " << times[i] << endl;
        }
    }
}

int main(int argc, char *argv[]) {
    if (argc != 2) {
        cerr << "Usage: ./check_camera_imu raw_data_folder" << endl;
        return -1;
    }

    // Load data
    vector<double> times, imuTimes;
    vector<cv::Point3f> vecAcc, vecGyro;
    const string dataFolder = argv[1];
    loadTimeStamps(dataFolder + "/times.txt", times);
    loadIMU(dataFolder + "/imu.txt", imuTimes, vecAcc, vecGyro);
    int n1 = (int) times.size(), n2 = (int) imuTimes.size();
    cout << "load " << n1 << " cameras, " << n2 << " imu data" << endl;

    cout << "Check camera time: " << endl;
    checkTimeStamps(times, 30);

    cout << endl;

    cout << "Check imu time: " << endl;
    checkTimeStamps(imuTimes, 400);

    return 0;
}

void loadTimeStamps(const string &path, vector<double> &timestamps) {
    ifstream fin(path);
    while (!fin.eof()) {
        string line;
        getline(fin, line);
        if (!line.empty()) {
            stringstream ss(line);
            double t;
            ss >> t;
            timestamps.push_back(t);
        }
    }
    fin.close();
}

void loadIMU(const string &path, vector<double> &timestamps, vector<cv::Point3f> &vecAcc,
             vector<cv::Point3f> &vecGyro) {
    ifstream fin(path);
    double last_time = 0;
    timestamps.reserve(5000), vecAcc.reserve(5000), vecGyro.reserve(5000);
    while (!fin.eof()) {
        string line;
        getline(fin, line);
        if (!line.empty()) {
            stringstream ss(line);
            double t;
            double data[6];
            ss >> t;
            if (t > last_time) {
                ss >> data[0] >> data[1] >> data[2] >> data[3] >> data[4] >> data[5];
                timestamps.push_back(t);
                vecAcc.emplace_back(data[0], data[1], data[2]);
                vecGyro.emplace_back(data[3], data[4], data[5]);
                last_time = t;
            }
        }
    }
    fin.close();
}